#include "headfile.h"
#include "main.h"
#include "drive.h"
#include "initialization.h"
#include "electromagnetic.h"
#include "pid.h"

#include "element.h"



extern uint16  ADC_AVDatap00;
extern uint16  ADC_AVDatap01;
extern uint16  ADC_AVDatap02;
extern uint16  ADC_AVDatap03;
extern uint16  ADC_AVDatap04;

extern uint16  ADC_TempDatap00;// 定义P00引脚的采集临时数据数组
extern uint16  ADC_TempDatap01;// 定义P01引脚的采集临时数据数组
extern uint16  ADC_TempDatap02;// 定义P02引脚的采集临时数据数组
extern uint16  ADC_TempDatap03;// 定义P03引脚的采集临时数据数组
extern uint16  ADC_TempDatap04;// 定义P04引脚的采集临时数据数组


	
uint16  ADC_MINDatap03;
uint16  ADC_MINDatap04;

uint16  ADC_AVDatap00;
uint16  ADC_AVDatap01;
uint16  ADC_AVDatap02;
uint16  ADC_AVDatap03;
uint16  ADC_AVDatap04;

uint16  ADC_TempDatap00;// 定义P00引脚的采集临时数据数组
uint16  ADC_TempDatap01;// 定义P01引脚的采集临时数据数组
uint16  ADC_TempDatap02;// 定义P02引脚的采集临时数据数组
uint16  ADC_TempDatap03;// 定义P03引脚的采集临时数据数组
uint16  ADC_TempDatap04;// 定义P04引脚的采集临时数据数组
uint16  ADC_DataSUM;// 定义5个电感总和数据的临时数据

uint16 ADC_Datap00;      // 定义P00引脚的归一化后数据数组
uint16 ADC_Datap01;      // 定义P01引脚的归一化后数据数组
uint16 ADC_Datap02;      // 定义P02引脚的归一化后数据数组
uint16 ADC_Datap03;      // 定义P03引脚的归一化后数据数组
uint16 ADC_Datap04;      // 定义P04引脚的归一化后数据数组

// 舵机速度闭环
static pid_type_def  S_Speed_Loop;//舵机速度环
const static fp32 SteeringSpeedLoop_PID[3] = {50, 0, 0};//舵机PID，P=50，I=0,D=0
void SteeringPID_Init(void)
{
	// 舵机控制闭环初始化
    PID_init(&S_Speed_Loop, PID_POSITION, SteeringSpeedLoop_PID, Steering_MAX_PWM, 0);
}


//电感数据采集
float InduSampling()
{
    static float  A = 2.5;            //A、B是加权参数，浙工大实验数据A=2.5,
    static float  B = 0.4;            //A、B是加权参数，浙工大实验数据B=0.4,
    static float  InduError;          // 定义电感误差
	  static float direction_controlout;

//获取电感值
			ADC_TempDatap00 = adc_once(ADC_P00,ADC_12BIT);//选择P00通道，采用12位分辨率，右1电感
			ADC_TempDatap01 = adc_once(ADC_P01,ADC_12BIT);//选择P01通道，采用12位分辨率，右2电感
			ADC_TempDatap02 = adc_once(ADC_P02,ADC_12BIT);//选择P02通道，采用12位分辨率，中间电感
			ADC_TempDatap03 = adc_once(ADC_P03,ADC_12BIT);//选择P03通道，采用12位分辨率，左2电感
			ADC_TempDatap04 = adc_once(ADC_P04,ADC_12BIT);//选择P04通道，采用12位分辨率，左1电感
      printf("%d,%d,%d,%d,%d.f\n",ADC_TempDatap00,ADC_TempDatap01,ADC_TempDatap02,ADC_TempDatap03,ADC_TempDatap04);			
	
				ADC_DataSUM = ADC_TempDatap00+ADC_TempDatap01+ADC_TempDatap02+ADC_TempDatap03+ADC_TempDatap04;//5个电感临时数据总和

//         if(ADC_DataSUM != 0)
//				 {
					 InduError = (1.6*(ADC_TempDatap04-ADC_TempDatap00)+0.4*(ADC_TempDatap03-ADC_TempDatap01))/(ADC_DataSUM);
//	PID_calc(&S_Speed_Loop, EncoVarLeft, 120);//EncoVarLeft反馈值为0；因为还没写编码器的函数
//				 }
//					else
//					{
//					 InduError = 0;
//					}
//			direction_controlout = steer_control(InduError);
//	  printf("%f.\n",InduError);	

		

		
//		 for(i = 0; i<10; i++)
//		 {
//			  //获取电感值然后从小到大排序
//				for ( j = 0; j < 9; j++)
//				{

//					if(ADC_TempDatap00[j]>ADC_TempDatap00[j+1])
//					{
//             	Temp = ADC_TempDatap00[j];
//						  ADC_TempDatap00[j] = ADC_TempDatap00[j+1];
//						  ADC_TempDatap00[j+1] = Temp;
//					}									
//					if(ADC_TempDatap01[j]>ADC_TempDatap01[j+1])
//					{
//             	Temp = ADC_TempDatap01[j];
//						  ADC_TempDatap01[j] = ADC_TempDatap01[j+1];
//						  ADC_TempDatap01[j+1] = Temp;
//					}

//					if(ADC_TempDatap02[j]>ADC_TempDatap02[j+1])
//					{
//             	Temp = ADC_TempDatap02[j];
//						  ADC_TempDatap02[j] = ADC_TempDatap02[j+1];
//						  ADC_TempDatap02[j+1] = Temp;
//					}

//						if(ADC_TempDatap03[j]>ADC_TempDatap03[j+1])
//					{
//             	Temp = ADC_TempDatap03[j];
//						  ADC_TempDatap03[j] = ADC_TempDatap03[j+1];
//						  ADC_TempDatap03[j+1] = Temp;
//					}
//					
//					if(ADC_TempDatap04[j]>ADC_TempDatap04[j+1])
//					{
//             	Temp = ADC_TempDatap04[j];
//						  ADC_TempDatap04[j] = ADC_TempDatap04[j+1];
//						  ADC_TempDatap04[j+1] = Temp;
//					}								
//				}
//			}
////		 
//      //去掉两边电感值获得最大值
//			ADC_MANDatap00 = ADC_TempDatap00[7];
//			ADC_MANDatap01 = ADC_TempDatap01[7];
//			ADC_MANDatap02 = ADC_TempDatap02[7];
//			ADC_MANDatap03 = ADC_TempDatap03[7];
//			ADC_MANDatap04 = ADC_TempDatap04[7];
//			//去掉两边电感值获得最小值
//			ADC_MINDatap00 = ADC_TempDatap00[2];
//			ADC_MINDatap01 = ADC_TempDatap01[2];
//			ADC_MINDatap02 = ADC_TempDatap02[2];
//			ADC_MINDatap03 = ADC_TempDatap03[2];
//			ADC_MINDatap04 = ADC_TempDatap04[2]; 
//			
////			
//			ADC_AVDatap00  = (ADC_TempDatap00[2]+ADC_TempDatap00[3]+ADC_TempDatap00[4]+ADC_TempDatap00[5]+ADC_TempDatap00[6]+ADC_TempDatap00[7])/6;
//			ADC_AVDatap01  = (ADC_TempDatap01[2]+ADC_TempDatap01[3]+ADC_TempDatap01[4]+ADC_TempDatap01[5]+ADC_TempDatap01[6]+ADC_TempDatap01[7])/6;
//			ADC_AVDatap02  = (ADC_TempDatap02[2]+ADC_TempDatap02[3]+ADC_TempDatap02[4]+ADC_TempDatap02[5]+ADC_TempDatap02[6]+ADC_TempDatap02[7])/6;
//			ADC_AVDatap03  = (ADC_TempDatap03[2]+ADC_TempDatap03[3]+ADC_TempDatap03[4]+ADC_TempDatap03[5]+ADC_TempDatap03[6]+ADC_TempDatap03[7])/6;
//			ADC_AVDatap04  = (ADC_TempDatap04[2]+ADC_TempDatap04[3]+ADC_TempDatap04[4]+ADC_TempDatap04[5]+ADC_TempDatap04[6]+ADC_TempDatap04[7])/6;
//		  printf("%d,%d,%d,%d,%d.\n",ADC_AVDatap00,ADC_AVDatap01,ADC_AVDatap02,ADC_AVDatap03,ADC_AVDatap04);				
////	


//				//再归一化
//				//标准数据 = （当前值 - 最小值）* 100 / （最大值 - 最小值）
//				ADC_Datap00 = ((float)(ADC_AVDatap00 - ADC_MINDatap00) / (float)(ADC_MANDatap00 - ADC_MINDatap00)) * 100.0;
//				//归一化限幅
//				ADC_Datap00 = ADC_Datap00 > 100 ? 100 : ADC_Datap00;
//				ADC_Datap00 = ADC_Datap00 < 0 ? 0 : ADC_Datap00;
//								//标准数据 = （当前值 - 最小值）* 100 / （最大值 - 最小值）
//				ADC_Datap01 = ((float)(ADC_AVDatap01 - ADC_MINDatap01) / (float)(ADC_MANDatap01 - ADC_MINDatap01)) * 100.0;
//				//归一化限幅
//				ADC_Datap01 = ADC_Datap01 > 100 ? 100 : ADC_Datap01;
//				ADC_Datap01 = ADC_Datap01 < 0 ? 0 : ADC_Datap01;
//								//标准数据 = （当前值 - 最小值）* 100 / （最大值 - 最小值）
//				ADC_Datap02 = ((float)(ADC_AVDatap02 - ADC_MINDatap02) / (float)(ADC_MANDatap02 - ADC_MINDatap02)) * 100.0;
//				//归一化限幅
//				ADC_Datap02 = ADC_Datap02 > 100 ? 100 : ADC_Datap02;
//				ADC_Datap02 = ADC_Datap02 < 0 ? 0 : ADC_Datap02;
//								//标准数据 = （当前值 - 最小值）* 100 / （最大值 - 最小值）
//				ADC_Datap03 = ((float)(ADC_AVDatap03 - ADC_MINDatap03) / (float)(ADC_MANDatap03 - ADC_MINDatap03)) * 100.0;
//				//归一化限幅
//				ADC_Datap03  = ADC_Datap03  > 100 ? 100 : ADC_Datap03 ;
//				ADC_Datap03  = ADC_Datap03  < 0 ? 0 : ADC_Datap03;
//								//标准数据 = （当前值 - 最小值）* 100 / （最大值 - 最小值）
//				ADC_Datap04 = ((float)(ADC_AVDatap04 - ADC_MINDatap04) / (float)(ADC_MANDatap04 - ADC_MINDatap04)) * 100.0;
//				//归一化限幅
//				ADC_Datap04 = ADC_Datap04 > 100 ? 100 : ADC_Datap04;
//				ADC_Datap04 = ADC_Datap04 < 0 ? 0 : ADC_Datap04;
////				printf("%d,%d,%d,%d,%d.\n",ADC_Datap00,ADC_Datap01,ADC_Datap02,ADC_Datap03,ADC_Datap04);				
//			
//	

//			
//			
//		
//				ADC_DataSUM = ADC_Datap00+ADC_Datap01+ADC_Datap02+ADC_Datap03+ADC_Datap04;//5个电感临时数据总和
//         if(ADC_DataSUM != 0)
//				 {
//					 InduError = (1.6*(ADC_Datap04-ADC_Datap00)+0.4*(ADC_Datap03-ADC_Datap01))/(ADC_DataSUM);
//				 }
//					else
//					{
//					 InduError = 0;
//					}
////					printf("%f.\n",InduError);
//			
//				direction_controlout = steer_control(InduError);
//				printf("%f.\n",direction_controlout);
		

//计算误差
    return InduError;

}

